﻿using System;
using System.Windows.Forms;
using System.IO;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using OpenCvSharp;
using CsGL.OpenGL;

namespace testyOpenCV
{
  public class ImageBW
  {
    public static IplImage lImg;
    public static IplImage rImg;
    public static IplImage someImg;
		public static int dmax;
		public static int recH;
		public static int recW;
		public static int wartMed;
		//public Cuboid[] cubs;
		//public float[] drawCubesVector;
		//public OurView drawCubes;

		//***************************************************************************************************


		public static CvScalar pixL = new CvScalar();
		public static CvScalar pixR = new CvScalar();
		public int control = 1;// = 255 / dmax;
		//public static int dmax = 15;   // max serching disp
		//static int recW = 7;    // szerokosc dopasowania
		//static int recH = 3;    // wysokosc dopasowania
		//public static int dmax = Image.dmaxIpl;   // max serching disp
		//static int recW = Image.winWidth;    // szerokosc dopasowania
		//static int recH = Image.winHeight;    // wysokosc dopasowania
		//static int wartMed = Image.medWin; // wielkosc okna filtru medianowego
		
		int LGT = 10;   // Line Growing Treshold
		int alfa = 1;
		int foc = 15;
		float m = 6;   // length between cameras
		int maxM, maxN;   // image resolution 
		public int[,] disp;
		public uint top = 0;
		public int state = 0;
		public int tempI;
		public int tempJ;
		public int tempIk;
		public int tempJw;
		public float errorE;    // energy error
		public IplImage dispIm = someImg;
		public IplImage depthMapIm = someImg;
		public static IplImage dispMedIm = someImg;
		//public static IplImage lIm = lImg.Clone();
		//public static IplImage rIm = rImg.Clone();

		public uint topLam = 0;
		public float vE = 0;
		//***************************************************************************************************
    //konstruktor pobieranych zdjec lewego i prawego
    public ImageBW(string fileNameL, string fileNameR, int dmaxIplT, int winHeightT, int winWidthT, int medWinT)
    {
			dmax = dmaxIplT;
			recH = winHeightT;
			recW = winWidthT;
			wartMed = medWinT;
			int recCentW = (recW / 2) + 1;
			int recNCentW = (recW / 2);
			int recCentH = (recH / 2) + 1;
			int recNCentH = (recH / 2);
      try
      {
        lImg = new IplImage(fileNameL);
        rImg = new IplImage(fileNameR);
      }
      catch (FileNotFoundException ex)
      {
        MessageBox.Show("Błędna nazwa pliku.","Błąd");
      }

			//************************************************************************************************
			IplImage grayImL = Cv.CreateImage(lImg.Size, BitDepth.U8, 1);
      Cv.CvtColor(lImg, grayImL, ColorConversion.BgrToGray);

      IplImage grayImR = Cv.CreateImage(rImg.Size, BitDepth.U8, 1);
      Cv.CvtColor(rImg, grayImR, ColorConversion.BgrToGray);

      IplImage dispIm = Cv.CreateImage(rImg.Size, BitDepth.U8, 1);
      Cv.CvtColor(rImg, dispIm, ColorConversion.BgrToGray);

      IplImage dispMedIm = Cv.CreateImage(rImg.Size, BitDepth.U8, 1);
      Cv.CvtColor(rImg, dispMedIm, ColorConversion.BgrToGray);

      maxM = lImg.Height;
      maxN = lImg.Width;

			

			//float[,] drawCubesVector = new float[maxM*maxN,3];
      uint[,] bL = new uint[maxM, maxN];   //matrix with colour value
      uint[,] gL = new uint[maxM, maxN];
      uint[,] rL = new uint[maxM, maxN];
      uint[,] bR = new uint[maxM, maxN];   //matrix with colour value
      uint[,] gR = new uint[maxM, maxN];
      uint[,] rR = new uint[maxM, maxN];
      int[,] lPix = new int[maxM, maxN];
      int[,] rPix = new int[maxM, maxN];
      disp = new int[maxM, maxN];
      uint[,] errDisp = new uint[maxM, maxN];
      float[,] edis = new float[maxM, maxN];
      int[,] regMap = new int[maxM, maxN];
      float[,] depthMap = new float[maxM, maxN];

      for (int i = 0; i < maxM; i++)   // loop fill-up a matrix  ???????????????
      {
        for (int j = 0; j < maxN; j++)
        {
          pixL = lImg.Get2D(i, j);
          lPix[i, j] = (int)pixL.Val0;

          pixR = rImg.Get2D(i, j);
          rPix[i, j] = (int)pixR.Val0;
                    
          disp[i, j] = 0;
          dispIm.Set2D(i, j, CvScalar.ScalarAll(0));   
				}
			}

			for (int m = (0 + recNCentW); m < (maxM - recNCentW); m++)
			{
				for (int n = (dmax + recNCentW); n < (maxN - recNCentW); n++)
				{
					double prevSum = 10000;
					for (int d = 0; d <= dmax; d++)
					{
						double sum = 0;
						for (int i = (m - recNCentH); i <= (m + recNCentH)  ; i++)
						{
							for (int j = (n - recNCentW); j <= (n + recNCentW); j++)
							{
								int diff = lPix[i, j] - rPix[i, j - d];
								sum += diff * diff;
								tempI = i;
								tempJ = j;
                                
							}
						}
						if (sum < prevSum)
						{
							disp[tempI, tempJ] = d;
							if (d > control) control = d;
							prevSum = sum;
						}
					}
				}
			}
			int number = 0;
      control = 255 / control;
      for (int m = 0; m < (maxM); m++)
      {
        for (int n = 0; n < (maxN); n++)
        {
					
          int a = disp[m, n];
          int temp = control * a;
					//cubs[number] = new Cuboid(n, m, temp);
					//drawCubesVector[number, 0] = m;
					//drawCubesVector[number, 1] = n;
					//drawCubesVector[number, 2] = a;
					//drawCubes.DrawCube(n, m, a, 0.1f);
					// TODO: albo jak powyzej i wywolac w ourview w petli, albo dostac sie do drawcube w ourview
					number++;
          dispIm.Set2D(m, n, CvScalar.ScalarAll(temp));
        }
      }

      Cv.Smooth(dispIm, dispMedIm, SmoothType.Median, wartMed, wartMed);

      dispMedIm.SaveImage("result_medianBW.png");
      dispIm.SaveImage("resultBW.png");

      //using (CvWindow window_DispImg = new CvWindow("DepthMapBW", dispIm))
      //using (CvWindow window_DispMedImg = new CvWindow("DepthMedMapBW", dispMedIm))
      //{
        //CvWindow.WaitKey();
      //}

			//************************************************************************************************
            
    }
		public void drawMyselfOGL() {
			DrawCube(0f, 0f, 0f, 0.1f);
			DrawCube(1f, 1f, 0, 0.6f);
		}
		public void DrawCube(float X, float Y, float Z, float size)
		{
			GL.glPushMatrix();

			GL.glTranslatef(X, Y, Z);

			GL.glPushMatrix();
			scianaXY(size);
			GL.glPopMatrix();

			GL.glPushMatrix();
			scianaXZ(size);
			GL.glPopMatrix();

			GL.glPushMatrix();
			scianaYZ(size);
			GL.glPopMatrix();

			GL.glPopMatrix();
		}
		public void scianaXY(float size)
		{
			GL.glBegin(GL.GL_QUADS); // Begin Drawing Triangles
			GL.glColor3f(0.0f, 1.0f, 0.0f);
			GL.glVertex3f(0.0f, 0.0f, 0.0f);
			GL.glVertex3f(size, 0.0f, 0.0f);
			GL.glVertex3f(size, size, 0.0f);
			GL.glVertex3f(0.0f, size, 0.0f);
			GL.glEnd();

			GL.glTranslatef(0.0f, 0.0f, size);
			GL.glBegin(GL.GL_QUADS); // Begin Drawing Triangles
			GL.glColor3f(0.0f, 1.0f, 0.0f);
			GL.glVertex3f(0.0f, 0.0f, 0.0f);
			GL.glVertex3f(size, 0.0f, 0.0f);
			GL.glVertex3f(size, size, 0.0f);
			GL.glVertex3f(0.0f, size, 0.0f);
			GL.glEnd();
		}

		public void scianaXZ(float size)
		{
			GL.glBegin(GL.GL_QUADS); // Begin Drawing Triangles
			GL.glColor3f(0.0f, 1.0f, 0.0f);
			GL.glVertex3f(0.0f, 0.0f, 0.0f);
			GL.glVertex3f(size, 0.0f, 0.0f);
			GL.glVertex3f(size, 0.0f, size);
			GL.glVertex3f(0.0f, 0.0f, size);
			GL.glEnd();

			GL.glTranslatef(0.0f, size, 0.0f);
			GL.glBegin(GL.GL_QUADS); // Begin Drawing Triangles
			GL.glColor3f(0.0f, 1.0f, 0.0f);
			GL.glVertex3f(0.0f, 0.0f, 0.0f);
			GL.glVertex3f(size, 0.0f, 0.0f);
			GL.glVertex3f(size, 0.0f, size);
			GL.glVertex3f(0.0f, 0.0f, size);
			GL.glEnd();
		}

		public void scianaYZ(float size)
		{
			GL.glBegin(GL.GL_QUADS); // Begin Drawing Triangles
			GL.glColor3f(0.0f, 1.0f, 0.0f);
			GL.glVertex3f(0.0f, 0.0f, 0.0f);
			GL.glVertex3f(0.0f, size, 0.0f);
			GL.glVertex3f(0.0f, size, size);
			GL.glVertex3f(0.0f, 0.0f, size);
			GL.glEnd();

			GL.glBegin(GL.GL_QUADS); // Begin Drawing Triangles
			GL.glColor3f(0.0f, 1.0f, 0.0f);
			GL.glVertex3f(0.0f, 0.0f, 0.0f);
			GL.glVertex3f(0.0f, size, 0.0f);
			GL.glVertex3f(0.0f, size, size);
			GL.glVertex3f(0.0f, 0.0f, size);
			GL.glEnd();
		}
    public ImageBW(string fileName)
    {
      try
      {
        someImg = new IplImage(fileName);
      }
      catch (FileNotFoundException ex2)
      {
      }

		}
  }
}
